//
// Created by iflyings on 6/30/23.
//

#ifndef _MB2_FIFO_H
#define _MB2_FIFO_H

#include "../../../../SystemUtils.h"

#define GET_INDEX(x)                                    ((x) & (mBufferSize - 1))

static inline bool is_power_of_2(uint32_t n)
{
    return (n != 0 && ((n & (n - 1)) == 0));
}
static __always_inline int fls(unsigned int x)
{
    int r = 32;

    if (!x)
        return 0;
    if (!(x & 0xffff0000u)) {
        x <<= 16;
        r -= 16;
    }
    if (!(x & 0xff000000u)) {
        x <<= 8;
        r -= 8;
    }
    if (!(x & 0xf0000000u)) {
        x <<= 4;
        r -= 4;
    }
    if (!(x & 0xc0000000u)) {
        x <<= 2;
        r -= 2;
    }
    if (!(x & 0x80000000u)) {
        x <<= 1;
        r -= 1;
    }
    return r;
}
static inline uint32_t roundup_pow_of_two(uint32_t x)
{
    return 1UL << fls(x - 1);
}

template<typename T>
class FifoBuffer {
public:
    FifoBuffer(int capacity)
    {
        mBufferSize = capacity;
        if (!is_power_of_2(mBufferSize)) {
            mBufferSize = roundup_pow_of_two(mBufferSize);
        }

        mInPosition = 0;
        mOutPosition = 0;
        mBuffer = new T[mBufferSize];
    }
    ~FifoBuffer()
    {
        delete[] mBuffer;
    }

    void clear()
    {
        mOutPosition = mInPosition;
    }
    void remove(uint32_t len = 1)
    {
        len = min(len, size());
        mOutPosition += len;
    }
    T& get(int index)
    {
        return mBuffer[GET_INDEX(index + mOutPosition)];
    }
    T& operator[](int index)
    {
        return mBuffer[GET_INDEX(index + mOutPosition)];
    }
    inline uint32_t size()
    {
        return mInPosition - mOutPosition;
    }

    uint32_t put(const T *buffer, uint32_t len)
    {
        len = min(len, mBufferSize - size());

        /*
        * Ensure that we sample the fifo->out index -before- we
        * start putting bytes into the kfifo.
        */

        /* first put the data starting from fifo->in to buffer end */
        uint32_t l = min(len, mBufferSize - GET_INDEX(mInPosition));
        memcpy(mBuffer + GET_INDEX(mInPosition), buffer, l);

        /* then put the rest (if any) at the beginning of the buffer */
        memcpy(mBuffer, buffer + l, len - l);

        /*
        * Ensure that we add the bytes to the kfifo -before-
        * we update the fifo->in index.
        */

        mInPosition += len;

       return len;
    }

    uint32_t get(T *buffer, uint32_t len)
    {
        len = min(len, size());

        /*
        * Ensure that we sample the fifo->in index -before- we
        * start removing bytes from the kfifo.
        */

        /* first get the data from fifo->out until the end of the buffer */
        uint32_t l = min(len, mBufferSize - GET_INDEX(mOutPosition));
        memcpy(buffer, mBuffer + GET_INDEX(mOutPosition), l);

        /* then get the rest (if any) from the beginning of the buffer */
        memcpy(buffer + l, mBuffer, len - l);

        /*
        * Ensure that we remove the bytes from the kfifo -before-
        * we update the fifo->out index.
        */

        mOutPosition += len;

        return len;
    }
private:
    uint32_t mBufferSize;
    uint32_t mInPosition;
    uint32_t mOutPosition;
    T *mBuffer;
};

#endif //_MB2_FIFO_H
